import json
# from PyQt5.Qt import *
from PyQt5.Qt import QThread
from PyQt5.QtGui import QImage, QPixmap
from PyQt5.QtCore import pyqtSignal,Qt, QPoint, QRect
from PyQt5 import QtCore
import cv2, time, imutils, logging, math
import numpy as np
from cnocr import CnOcr
import pyzbar.pyzbar as pyzbar
import modbus_tk.defines as cst
from modbus_tk import modbus_tcp, hooks
import numpy as np

import robot_

logger = logging.getLogger(__name__)
logger.setLevel(logging.DEBUG)

kernel_2 = np.ones((2, 2), np.uint8)  # 2x2的卷积核
kernel_3 = np.ones((3, 3), np.uint8)  # 3x3的卷积核
kernel_4 = np.ones((4, 4), np.uint8)  # 4x4的卷积核
kernel_8 = np.ones((8, 8), np.uint8)

draw_color = (0,255,0)

Lower_red = np.array([0, 61, 136])    #red
Upper_red = np.array([5, 255, 255])
draw_red = draw_color
red = [Lower_red, Upper_red, 'red',draw_red]

Lower_blue = np.array([67, 91, 170])
Upper_blue = np.array([113, 193, 255])
draw_blue =draw_color
blue = [Lower_blue, Upper_blue, 'blue',draw_blue]

Lower_green = np.array([39, 60, 133])
Upper_green = np.array([96, 255, 255])
draw_green =draw_color
green = [Lower_green, Upper_green, 'green',draw_green]

Lower_yellow = np.array([21, 103, 63])   
Upper_yellow = np.array([37, 255, 255])
draw_yellow = draw_color
yellow = [Lower_yellow, Upper_yellow, 'yellow',draw_yellow]

Lower_black = np.array([70, 100, 100])
Upper_black = np.array([87, 150, 120])
draw_black = (0,0,0)
black = [Lower_black, Upper_black, 'black',draw_black]

color_list = [red,blue,green,yellow]

camera_nun = 9

class Camera_Thread(QThread):

    cap_signal = pyqtSignal(str)

    def __init__(self,label,gui_signal):
        super().__init__()
        self.stop = False
        self.Is_exit = False
        self.cap= None
        self.frame = None
        self.program_flag = 0 # 标记程序运行索引

        self.label = label
        self.selected_area = None
        self.label.mouseMoveEvent = self.mouseMoveEvent
        self.label.mouseMoveEvent = self.mouseMoveEvent
        self.label.mouseReleaseEvent = self.mouseReleaseEvent

        self.gui_signal=gui_signal
        self.cap_signal.connect(self.cap_camera)

        self.count_pos = 0

        self.master = Manger_dis(self.gui_signal)
        self.master.start()
        self.position_init = {
                            "1":[], "2":[], "3":[], "4":[],
                            "A":[],"B":[], "C":[],"D":[],
                            "red":[],"blue":[],"green":[],"yellow":[],
                            "circle":[], "triangle":[],"rectangle":[],"hexagon":[],
                           }
        self.position = self.position_init     

    def exit(self):
        self.stop = True
        self.Is_exit= True

    #### 重写鼠标事件---实时获取图像HSV值 start
    def mousePressEvent(self, event): 
        if event.button() == Qt.LeftButton:  
            self.origin = event.pos()  
            self.selected_area = QRect(self.origin, QPoint())  
  
    def mouseMoveEvent(self, event):  
        print(2)
        # if not self.selected_area.isNone():  
        self.selected_area.setRight(event.x())  
        self.selected_area.setBottom(event.y())  
            # 在这里可以实时更新显示选区（如果需要）  

    def qimage_to_numpy(self,image): 
        if image.isNull():  
            return None  
    
        # 如果不是 RGB888 格式，则转换为 RGB888  
        if image.format() != QImage.Format_RGB888:  
            image = image.convertToFormat(QImage.Format_RGB888)  
    
        # 获取图像的宽度和高度  
        width = image.width()  
        height = image.height()  
    
        # 计算字节总数（每个像素3个字节）  
        bytes_per_line = image.bytesPerLine()  
        buffer_size = height * bytes_per_line  
    
        # 创建一个空的 NumPy 数组来存储像素数据  
        buf = np.zeros((height, width, 3), np.uint8)  
    
        # 使用 QImage 的 bits 和 bytesPerLine 来进行数据拷贝  
        # QImage 可能会有行对齐，所以不能直接使用 width * 3 * height  
        qimage_ptr = image.constBits()  
        qimage_ptr.setsize(buffer_size)  
    
        # 逐行拷贝数据  
        for y in range(height):  
            # 注意：这里假设 QImage 是以 BGR 格式存储的  
            buf[y, :, :] = np.frombuffer(qimage_ptr, np.uint8, count=width*3, offset=y*bytes_per_line).reshape(width, 3)[:, ::-1]  # BGR to RGB  
    
        return buf 

    def mouseReleaseEvent(self, event):  
        if not self.selected_area.isNull():  
            # 从 QLabel 获取 QPixmap，并转换为 QImage（如果需要）  
            pixmap = self.label.pixmap()  
            image = pixmap.toImage()  
  
            # 将 QImage 转换为 OpenCV 格式并获取 HSV  
            rgb_image = self.qimage_to_numpy(image)  
            hsv_image = cv2.cvtColor(rgb_image, cv2.COLOR_BGR2HSV)  # 注意 OpenCV 使用 BGR  
  
            # 计算选区内的 HSV 最大值和最小值  
            hsv_values = hsv_image[self.selected_area.y():self.selected_area.bottom(),  
                                   self.selected_area.x():self.selected_area.right()]
            
            cv2.rectangle(rgb_image, (self.selected_area.x(), self.selected_area.y()), (self.selected_area.right(), self.selected_area.bottom()), (0, 0, 255), 2)
            
            # 获取HSV三个通道各自的最大值和最小值
            h_min, s_min, v_min = np.min(hsv_values, axis=(0, 1))
            h_max, s_max, v_max = np.max(hsv_values, axis=(0, 1))

            self.gui_signal.emit(json.dumps({"source":6,"name":f"HSV区域最值:\nH_min={h_min}, S_min={s_min}, V_min={v_min} H_max={h_max}, S_max={s_max}, V_max={v_max}"}))


            # print(f"HSV区域最值:\nH_min={h_min}, S_min={s_min}, V_min={v_min}")
            # print(f"H_max={h_max}, S_max={s_max}, V_max={v_max}")  
    
    #### 重写鼠标事件---实时获取图像HSV值 end    

    #### APP 
    def ocr_test(self,img):
        try:
            ####设置识别中英文两种语言 need to run only once to load model into memory
            ocr = CnOcr()
            ress = ocr.ocr(img)
            if ress:
                for res in ress:
                    text = res['text']
                    position = res['position']

                    x1, y1 = int(position[0][0]), int(position[0][1])
                    x2, y2 = int(position[2][0]), int(position[2][1])

                    center_x = int((x1 + x2)/2)
                    center_y = int((y1 + y2)/2)
                    cv2.circle(self.frame, (center_x, center_y), 7, (125, 125, 0), -1)
                    feature = [center_x,center_y,0]

                    cv2.rectangle(self.frame, (x1,y1),(x2,y2), (255, 0, 0), 3)  
                    match text:
                        case "A" | "4" | "^" | "Y" | "人" |"a":
                            self.position["A"] = feature
                            cv2.putText(self.frame, "A", (int(x1),int(y1)-10), cv2.FONT_HERSHEY_SIMPLEX, 4, (0,0,255), 4)
                        case "B" | "8" | "b":
                            self.position["B"] = feature
                            cv2.putText(self.frame, "B", (int(x1),int(y1)-10), cv2.FONT_HERSHEY_SIMPLEX, 4, (0,0,255), 4)
                        case "C"| "c":
                            self.position["C"] = feature
                            cv2.putText(self.frame, "C", (int(x1),int(y1)-10), cv2.FONT_HERSHEY_SIMPLEX, 4, (0,0,255), 4)
                        case "D" | "0" | "O" | "o" | "d":
                            self.position["D"] = feature
                            cv2.putText(self.frame, "D", (int(x1),int(y1)-10), cv2.FONT_HERSHEY_SIMPLEX, 4, (0,0,255), 4)                      
                    print("识别到的字符为：%s" %text)
                    # cv2.putText(self.frame, text, (int(x1),int(y1)-10), cv2.FONT_HERSHEY_SIMPLEX, 5, (0,0,255), 2)

        except Exception as e:
            logger.error(e,exc_info=True)

    def QR_test(self,img):
        try:
            # 使用pyzbar解析二维码图片内容
            barcodes = pyzbar.decode(img)
            if barcodes:
                for barcode in barcodes:
                    # 提取条形码的边界框的位置
                    # 画出图像中条形码的边界框
                    (x, y, w, h) = barcode.rect
                    cv2.rectangle(self.frame, (x, y), (x + w, y + h), (0, 255, 255), 3)
            
                    # 条形码数据为字节对象，所以如果我们想在输出图像上画出来，就需要先将它转换成字符串  类型也直接识别出来了
                    barcodeData = barcode.data.decode("utf-8")
                    barcodeType = barcode.type

                    center_x = int(x+w/2)
                    center_y = int(y+h/2)
                    feature = [center_x,center_y,0]
                    match barcodeData:
                        case "1":
                            self.position["1"] = feature
                        case "2":
                            self.position["2"] = feature
                        case "3":
                            self.position["3"] = feature
                        case "4":
                            self.position["4"] = feature

                    # 绘出图像上条形码的数据和条形码类型
                    text = "{} ({})".format(barcodeData, barcodeType)
                    cv2.putText(self.frame, text, (x, y - 10), cv2.FONT_HERSHEY_SIMPLEX, 4, (0, 0, 125), 4)            

        except Exception as e:
            logger.error(e,exc_info=True)

    def waining_angle(self,):
        try:
            pass
        except Exception as e:
            logger.error(e,exc_info=True)

    def triangle_angle(self,approx):
        try:
            approx = approx.tolist()
            centroid = [(approx[0][0][0] + approx[1][0][0] + approx[2][0][0]) / 3,
                        (approx[0][0][1] + approx[1][0][1] + approx[2][0][1]) / 3]
            distances = [math.sqrt(math.pow(vertex[0][0] - centroid[0], 2) + math.pow(vertex[0][1] - centroid[1], 2)) for vertex in approx]
            max_distance = max(distances)
            max_index = distances.index(max_distance)
            angle_rad = math.atan2(approx[max_index][0][1] - centroid[1], approx[max_index][0][0] - centroid[0])
            angle_deg = math.degrees(angle_rad)
            self.master.master.execute(1, cst.WRITE_SINGLE_REGISTER, 9, output_value=int(angle_deg))
            rema = angle_deg % 120
            if rema  < 60:
                rema = rema
            elif rema > 60 and rema < 120:
                rema = rema -120
            return int(rema)

        except Exception as e:
            print("Error:", e)
            return None

    def hexagon_angle(self,approx):
        try:
            approx = approx.tolist()

            # 计算中心点
            centroid = [(approx[0][0][0] + approx[1][0][0] + approx[2][0][0] + approx[3][0][0] + approx[4][0][0] + approx[5][0][0]) / 6,
                        (approx[0][0][1] + approx[1][0][1] + approx[2][0][1] + approx[3][0][1] + approx[4][0][1] + approx[5][0][1]) / 6]

            # 计算顶点到中心的距离
            distances = [math.sqrt(math.pow(vertex[0][0] - centroid[0], 2) + math.pow(vertex[0][1] - centroid[1], 2)) for vertex in approx]

            # 找到距离最远的顶点
            max_distance = max(distances)
            max_index = distances.index(max_distance)

            # 计算旋转角度
            angle_rad = math.atan2(approx[max_index][0][1] - centroid[1], approx[max_index][0][0] - centroid[0])
            angle_deg = math.degrees(angle_rad)

            rema = angle_deg % 60
            if rema < 30:
                rema = rema
            elif  rema > 30 and rema < 60:
                rema = rema - 60
            return int(rema)

        except Exception as e:
            print(f"Error: {e}")
            return None

    # 计算角度
    def angel_count(self,cnt_max):
        try:
            peri = cv2.arcLength(cnt_max,True)
            # 获取轮廓顶点
            approx = cv2.approxPolyDP(cnt_max,0.04*peri,True)   
            point_num =  len(approx)
            

            # 最小的外接矩形
            rect = cv2.minAreaRect(cnt_max)
            # 获取最小外接矩形的4个顶点
            box = cv2.boxPoints(rect)  
            box = np.int0(box)
            theta = 0
            if 0 not in box.ravel():
                # 旋转角度
                theta = cv2.minAreaRect(cnt_max)[2]
                # 三角形的单独计算角度
                # if point_num == 3:     
                #     theta = self.triangle_angle(approx)
                # # 其余角度全部按外接四边形方法
                # else:
                
                # 获取宽度和高度，第一个是宽度   
                width, height = cv2.minAreaRect(cnt_max)[1]   
                if width < height :
                    theta = theta+90
                    if int(-(180 - theta)) < -69:
                        theta = theta + 90
                # #### 逆时针方向
                elif width > height:
                    theta = 180 + theta
                # print('图片的旋转角度为%s.'%theta)
            return int(-(180- theta))

        except Exception as e:
            logger.error(e,exc_info=True)

    # 计算角度
    def angel_count(self,cnt_max):
        try:
            peri = cv2.arcLength(cnt_max,True)
            approx = cv2.approxPolyDP(cnt_max,0.04*peri,True)   # 获取轮廓顶点
            point_num =  len(approx)

            # 最小的外接矩形
            rect = cv2.minAreaRect(cnt_max)
            box = cv2.boxPoints(rect)  # 获取最小外接矩形的4个顶点
            box = np.int0(box)
            theta = 0
            if 0 not in box.ravel():
                # 旋转角度
                theta = cv2.minAreaRect(cnt_max)[2]

                if point_num == 3:     # 三角形的单独计算角度
                    theta = self.triangle_angle(approx)
                
                else:     # 其余角度全部按外接四边形方法
                    width, height = cv2.minAreaRect(cnt_max)[1]   # 获取宽度和高度，第一个是宽度
                    if width < height:
                        theta = theta+90

                # print('图片的旋转角度为%s.'%theta)
            return int(180-theta)

        except Exception as e:
            logger.error(e,exc_info=True)

    # 根据轮廓画出框框
    def draw_rect(self,radium,cnt_max):
        # 画圆的圆心、半径
        x, y, r = int(radium[0]), int(radium[1]), int(radium[2])
        cv2.circle(self.frame, (x, y), r, (0, 0, 0), 3)

    # 测试出最大的轮廓
    def max_contour(self,radium,contours,name):
        are_max = 30   # 定义一个初始的轮廓最大面积
        cnt_max = None   # 存储一个最大的轮廓
        flag = 0
        feature = []
        for cnt in contours:
            self.area = cv2.contourArea(cnt)
            # 最小外界矩形的宽度和高度
            if self.area > are_max:
                flag = 1
                are_max = self.area
                cnt_max = cnt
        if flag:
            angle = self.angel_count(cnt_max)
            cX = int(radium[0])
            cY = int(radium[1])
            match name:
                case "red":
                    name = "Harmful"
                case "green":
                    name = "Kitchen"
                case "blue":
                    name = "Recycle"
                case "yellow":
                    name = "Other"

            self.draw_rect(radium,cnt_max=cnt_max)
            cv2.circle(self.frame, (cX , cY ), int(radium[2]), (0, 0, 0), 1)
            cv2.putText(self.frame, name, (cX - 20, cY - 20),cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 2)  
            
            feature.extend([cX,cY])
            feature.append(angle)

        return feature

    #### 形状识别
    def contour_recong(self,frame):
        max_area = 30000
        min_area = 3000
        # HSV = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
        # color = black
        # mask_= cv2.inRange(HSV, color[0], color[1])
        # ## 腐蚀  膨胀
        # erosion = cv2.erode(mask_, kernel_2, iterations=1)   
        # dilation = cv2.dilate(erosion, kernel_4, iterations=1)  
        # # 二值化，将滤波后的图像变成二值图像放在binary中
        # ret, binary = cv2.threshold(dilation, 0, 255, cv2.THRESH_BINARY) 
        # cnts = cv2.findContours(binary.copy(), cv2.RETR_EXTERNAL,
        #                         cv2.CHAIN_APPROX_SIMPLE)
        # contours = imutils.grab_contours(cnts)
        # 进行高斯模糊操作
        blurred = cv2.GaussianBlur(frame, (5, 5), 0)
        # 进行图片灰度化
        gray = cv2.cvtColor(blurred, cv2.COLOR_BGR2GRAY)
        # 进行阈值分割
        thresh = cv2.threshold(gray, 130, 255, cv2.THRESH_BINARY)[1]
        # 在二值图片中寻找轮廓
        contours = cv2.findContours(thresh.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
        contours = imutils.grab_contours(contours)
        if contours is not None:
            for cnt in contours:  
                # 计算轮廓的边界框  
                x, y, w, h = cv2.boundingRect(cnt)
                contour_area = cv2.contourArea(cnt)
                if contour_area > min_area and contour_area < max_area:
                    cv2.rectangle(frame, (x, y), (x+w, y+h), (0, 255, 0), 3)
                    approx = cv2.approxPolyDP(cnt, 0.016 * cv2.arcLength(cnt, True), True)
                    n_corners = len(approx)
                    # 三角形：三个顶点  
                    if n_corners == 3:
                        angle = self.triangle_angle(approx)
                        M = cv2.moments(cnt)
                        center_tri = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
                        cv2.drawContours(self.frame, [approx], -1, (0, 255, 0), 2)
                        self.position["triangle"] = [center_tri[0], center_tri[1], angle]
                        cv2.putText(self.frame, "triangle", (int(center_tri[0]),int(center_tri[1])-10), cv2.FONT_HERSHEY_SIMPLEX, 2, (0,0,255), 3)
               
                    # 正方形：接近正方形的长宽比和四个直角  
                    elif n_corners == 4: 
                        angle = self.angel_count(cnt)
                        if abs(cv2.contourArea(cnt) / ((w*h) ** 0.5)) > 0.95:    
                            M = cv2.moments(cnt)
                            center_square = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
                            cv2.drawContours(self.frame, [approx], -1, (0, 255, 0), 2)
                            self.position["rectangle"] = [center_square[0], center_square[1], angle]
                            cv2.putText(self.frame, "rectangle", (int(center_square[0]),int(center_square[1])-10), cv2.FONT_HERSHEY_SIMPLEX, 2, (0,0,255), 3)

                    # 多边形：
                    elif n_corners >4 and n_corners < 8: 
                        angle = self.hexagon_angle(approx)
                        M = cv2.moments(cnt)
                        center_hexagon = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
                        cv2.drawContours(self.frame, [approx], -1, (0, 255, 0), 2)
                        self.position["hexagon"] = [center_hexagon[0], center_hexagon[1], angle]
                        cv2.putText(self.frame, "hexagon", (int(center_hexagon[0]),int(center_hexagon[1])-10), cv2.FONT_HERSHEY_SIMPLEX, 2, (0,0,255), 3)
    
                    # 圆形：近似为圆形的轮廓  
                    elif n_corners > 7 and n_corners < 100: 
                        angle = self.angel_count(cnt)  
                        M = cv2.moments(cnt)
                        center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
                        cv2.drawContours(self.frame, [approx], -1, (0, 255, 0), 2)
                        self.position["circle"] = [center[0], center[1], angle]
                        cv2.putText(self.frame, "circle", (int(center[0]),int(center[1])-10), cv2.FONT_HERSHEY_SIMPLEX, 2, (0,0,255), 3)

    #### 颜色识别
    def color_recong(self,frame):
        #### 转换为灰度图像
        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        #### 应用高斯模糊
        blurred = cv2.GaussianBlur(gray, (5, 5), 0)
        #### 检测圆形
        circles = cv2.HoughCircles(blurred, cv2.HOUGH_GRADIENT, 1.2, 100)
        if circles is not None:
            for circle_ in circles[0]:
                mask1 = np.zeros_like(frame)
                x, y, r = int(circle_[0]), int(circle_[1]), int(circle_[2])
                inner_radius = r + 10
                #### 绘制大圆
                cv2.circle(mask1, (x, y), inner_radius, (255, 255, 255), -1)
                outer_circle = cv2.bitwise_and(frame, mask1)
                mask1 = np.zeros_like(frame)
                #### 绘制小圆        
                cv2.circle(mask1, (x, y), r, (255, 255, 255), -1)
                inner_circle_inverse = cv2.bitwise_not(mask1)
                annulus_area = cv2.bitwise_and(outer_circle, inner_circle_inverse)
                
                ##### 把BGR图像转换为HSV格式
                HSV = cv2.cvtColor(annulus_area, cv2.COLOR_BGR2HSV)  
                for color in color_list:
                    mask_red = cv2.inRange(HSV, color[0], color[1])
                    #### 腐蚀 膨胀
                    erosion = cv2.erode(mask_red, kernel_2, iterations=1)   
                    dilation = cv2.dilate(erosion, kernel_4, iterations=1)  
                    #### 二值化，将滤波后的图像变成二值图像放在binary中
                    ret, binary = cv2.threshold(dilation, 0, 255, cv2.THRESH_BINARY)  
                    cnts = cv2.findContours(binary.copy(), cv2.RETR_EXTERNAL,
                                            cv2.CHAIN_APPROX_NONE)
                    cnts = imutils.grab_contours(cnts) 
                    self.max_contour(circle_,cnts,color[2])
                    #### 更新位置
                    self.position[color[2]] = [int(circle_[0]), int(circle_[1]), 0]       

    #### 展示图片
    def display(self,):
        try:
            img  = cv2.resize(self.frame,(660,300))
            rgb_img=cv2.cvtColor(img,cv2.COLOR_BGR2RGB)
            #将rgb格式转换为qt可以识别的格式
            img_dis = QImage(rgb_img,rgb_img.shape[1],rgb_img.shape[0],QImage.Format_RGB888)
            # print(rgb_img.shape[1],rgb_img.shape[0])
            #加载图片，并设定图片大小
            img_dis = QPixmap(img_dis)
            #####显示图片# 将图片放到label中心
            self.label.setAlignment(QtCore.Qt.AlignCenter) 
            self.label.setPixmap(img_dis)
            time.sleep(0.001)
        except Exception as e:
            logger.error(e,exc_info=True)

    def run(self): 
        self.cap = cv2.VideoCapture(camera_nun)
        self.cap.set(cv2.CAP_PROP_FOURCC,cv2.VideoWriter_fourcc('M','J','P','G')) 

        while not self.Is_exit:
            while not self.stop:
                try:
                    # 获取摄像头捕获的画面帧，返回ret和frame
                    # ret的True/False反映是否捕获成功，frame是画面
                    ret, self.frame = self.cap.read()
                    if not ret:
                        self.gui_signal.emit(json.dumps({"source":2,}))
                        time.sleep(2)
                        self.cap = cv2.VideoCapture(camera_nun)
                        self.cap.set(cv2.CAP_PROP_FOURCC,cv2.VideoWriter_fourcc('M','J','P','G')) 
      
                    # 在窗口中显示处理之后的画面
                    if ret:
                        self.position  = self.position_init
                        h,w,c =self.frame.shape  
                        image = self.frame  
                        
                        #### 进行颜色检测识别--垃圾分拣
                        if self.program_flag ==0:
                            self.color_recong(image)
                        
                        #### 进行轮廓检测识别
                        elif self.program_flag == 1:
                            self.contour_recong(image)  
                        
                        #### 进行二维码识别
                        elif self.program_flag == 2:
                            self.QR_test(image)   
                        
                        #### 进行OCR检测识别
                        elif self.program_flag == 3:
                            self.ocr_test(image)  
                        
                        self.master.program_flag = self.program_flag
                        self.master.position = self.position
                        self.display()
                        time.sleep(0.1)
                        cv2.imwrite("img/output.png", self.frame) 
                except Exception as e:
                    logger.error(e,exc_info=True)
              
    def cap_camera(self,msg):
        try:
            message_json = json.loads(msg)
            source = int(message_json.get("source"))
            match source:
                case 1:
                    self.master.action_flag = 1
                case 2:
                    self.master.action_flag = 2
                case 3:
                    self.master.action_flag = 3
                case 4:
                    self.master.action_flag = 4
                case 5:
                    self.master.action_flag = 5
                case 6:
                    self.master.action_flag = 6
                case 7:
                    self.master.action_flag = 7
                case 8:
                    self.master.action_flag = 8
                case 9:
                    self.master.action_flag = 9
                case 11:
                    self.master.action_flag = 11
                case 12:
                    self.master.action_flag = 12
                case 13:
                    self.master.action_flag = 13
                case 14:
                    self.master.action_flag = 14
                case 15:
                    self.master.action_flag = 15
                case 16:
                    self.master.action_flag = 16
                case 17:
                    self.master.action_flag = 17
                case 19:
                    self.master.action_flag = 19
                case 20:
                    self.master.action_flag = 20
                case 21:
                    self.master.action_flag = 21
                case 22:
                    self.master.action_flag = 22
                case 23:
                    self.master.action_flag = 23
                case 24:
                    self.master.action_flag = 24
        except Exception as e:
            logger.error(e,exc_info=True)


class Manger_dis(QThread):

    def __init__(self,signal) :
        super().__init__()
        self.run_flag = 1
        self.gui_signal =signal
        self.action_flag = 0       # 定义动作
        self.auto_flag = 0         # 自动运行标志位
        self.position = None       # 

        self.camera_position = 0   #相机拍照位置
        self.home_position = 0     #机器人初始位置
        self.program_flag = 0       # 标记运行的程序
        self.auto_first = 0

        self.calibrate_4Points()
        
        self.robot = robot_.Robot_Driver()
        self.master_init()

    def master_init(self,):
        #### host主机名为本地IP
        self.master = modbus_tcp.TcpMaster(host= '127.0.0.1')
        self.master.set_timeout(5.0)

    ##### 此函数专门向服务器发送指令
    def tcp_send(self,value,cmd):
        try:
            self.robot.robot_Action(value, cmd)

        except Exception as e:
            logger.error(e,exc_info=True)

    def calibrate_4Points(self):
        # 坐标值  
        pixel_coords = np.array([[185, 176], [280, 371], [376, 82], [474, 273]])  
        robot_coords = np.array([[-110.876, 119.949], [-104.295, 187.144], [-48.897, 120.83], [-47.11, 186.4911]])  
        
        # 构建增广矩阵 (A | B)  
        A = np.hstack((pixel_coords, np.ones((pixel_coords.shape[0], 1))))  
        B = robot_coords  
        
        # 求解线性方程组  
        # 注意：lstsq返回的系数可能需要进行转置，因为lstsq返回的是最小二乘解，其形状可能不是期望的  
        self.coefficients, _, _, _ = np.linalg.lstsq(A, B, rcond=None)  
        
        # 因为lstsq返回的系数可能是列向量，我们需要转置它以匹配仿射变换的公式  
        self.coefficients = self.coefficients.T  


    ####此函数为了计算像素到物理位置的换算
    def count_position(self,value):
        # value：输入的是三个值
        try:
            robot_x = self.coefficients[0, 0] * value[0] + self.coefficients[0, 1] * value[1] + self.coefficients[0, 2]  
            robot_y = self.coefficients[1, 0] * value[0] + self.coefficients[1, 1] * value[1] + self.coefficients[1, 2]  
            angle = value[2]
            return [robot_x, robot_y, angle] 

        except Exception  as e:
            logger.error(e,exc_info=True)

    def wait_over(self,):
        try:
            if self.auto_flag:
                while self.master.execute(1,cst.READ_HOLDING_REGISTERS,starting_address=100,quantity_of_x=1)[0] <100:
                    time.sleep(0.5)
                if self.auto_flag:
                    self.action_flag +=1
                    time.sleep(1)
                    if self.program_flag == 2:
                        count = 0
                        while self.auto_flag:
                            time.sleep(1)
                            count +=1
                            if count == 12:
                                break
                else:
                    self.action_flag =0
            else:
                self.action_flag = 0
    
        except Exception as e:
            logger.error(e,exc_info=True)

    def action_run(self,num): 
        try:
            if self.position != None:
                 
                match num:
                    case 0:
                        pass
                    case 1:  # 移动二维码1
                        cmd = 21
                        posi = self.position["1"]
                        if posi !=[]:
                            posi = self.count_position(posi)
                            self.tcp_send(value=posi,cmd=cmd)
                            self.gui_signal.emit(json.dumps({"source":1,"name":"QR1"}))
                        else:
                            self.gui_signal.emit(json.dumps({"source":0,"name":"QR1"}))
                        self.wait_over()

                    case 2:  # 移动二维码2
                        cmd = 22
                        posi = self.position["2"]
                        if posi !=[]:
                            posi = self.count_position(posi)
                            self.tcp_send(value=posi,cmd=cmd)
                            self.gui_signal.emit(json.dumps({"source":1,"name":"QR2"}))
                        else:
                            self.gui_signal.emit(json.dumps({"source":0,"name":"QR2"}))
                        self.wait_over()

                    case 3:  # 移动二维码3
                        cmd = 23
                        posi = self.position["3"]
                        if posi !=[]:
                            posi = self.count_position(posi)
                            self.tcp_send(value=posi,cmd=cmd)
                            self.gui_signal.emit(json.dumps({"source":1,"name":"QR3"}))
                        else:
                            self.gui_signal.emit(json.dumps({"source":0,"name":"QR3"}))
                        self.wait_over()


                    case 4:  # 移动二维码4
                        cmd = 24
                        posi = self.position["4"]
                        if posi !=[]:
                            posi = self.count_position(posi)
                            self.tcp_send(value=posi,cmd=cmd)
                            self.gui_signal.emit(json.dumps({"source":1,"name":"QR4"}))
                        else:
                            self.gui_signal.emit(json.dumps({"source":0,"name":"QR4"}))
                        self.wait_over()
                        self.action_flag = 18
                        
                            
                    case 5:  # 移动字符A
                        cmd = 21
                        posi = self.position["A"]
                        if posi !=[]:
                            posi = self.count_position(posi)
                            self.tcp_send(value=posi,cmd=cmd)
                            self.gui_signal.emit(json.dumps({"source":1,"name":"A"}))
                        else:
                            self.gui_signal.emit(json.dumps({"source":0,"name":"A"}))
                        self.wait_over()

                    case 6:  # 移动字符B
                        cmd = 22
             
                        if self.position["B"] !=[]:
                            posi = self.count_position(posi)
                            self.tcp_send(value=self.position["B"],cmd=cmd)
                            self.gui_signal.emit(json.dumps({"source":1,"name":"B"}))
                        else:
                            self.gui_signal.emit(json.dumps({"source":0,"name":"B"}))
                        self.wait_over()


                    case 7:  # 移动字符C
                        cmd = 23
                        posi = self.position["C"]
                        if posi !=[]:
                            posi = self.count_position(posi)
                            self.tcp_send(value=posi,cmd=cmd)
                            self.gui_signal.emit(json.dumps({"source":1,"name":"C"}))
                        else:
                            self.gui_signal.emit(json.dumps({"source":0,"name":"C"}))
                        self.wait_over()

                    case 8:  # 移动字符D
                        cmd = 24
                        
                        if self.position["D"] !=[]:
                            posi = self.count_position(posi)
                            self.tcp_send(value=self.position["D"],cmd=cmd)
                            self.gui_signal.emit(json.dumps({"source":1,"name":"D"}))
                        else:
                            self.gui_signal.emit(json.dumps({"source":0,"name":"D"}))
                        self.wait_over()


                    # case 9:  # 移动字符E
                    #     cmd = 21
                    #     posi = self.position["E"]
                    #     if posi !=[]:
                    #         posi = self.count_position(posi)
                    #         self.tcp_send(value=posi,cmd=cmd)
                    #         self.gui_signal.emit(json.dumps({"source":1,"name":"E"}))
                    #     else:
                    #         self.gui_signal.emit(json.dumps({"source":0,"name":"E"}))
                    #     self.wait_over()
                    #     self.action_flag = 18

                    case 10:  # 过渡点
                        self.wait_over()


                    case 11:  # 移动七巧板
                        cmd = 11
                        posi = self.position["red"]
                        if posi !=[]:
                            posi = self.count_position(posi)
                            self.tcp_send(value=posi,cmd=cmd)
                            
                            self.gui_signal.emit(json.dumps({"source":1,"name":"HarmFul"}))
                        else:
                            self.gui_signal.emit(json.dumps({"source":0,"name":"HarmFul"}))
                        self.wait_over()
                    
                    case 12:  # 移动七巧板
                        cmd = 12
                        posi = self.position["blue"]
                        if posi !=[]:
                            posi = self.count_position(posi)
                            self.tcp_send(value=posi,cmd=cmd)
                            self.gui_signal.emit(json.dumps({"source":1,"name":"Recycle"}))
                        else:
                            self.gui_signal.emit(json.dumps({"source":0,"name":"Recycle"}))
                        self.wait_over()

                    case 13:  # 移动七巧板
                        cmd = num
                        posi = self.position["purple"]
                        if posi !=[]:
                            posi = self.count_position(posi)
                            self.tcp_send(value=posi,cmd=cmd)
                            self.gui_signal.emit(json.dumps({"source":1,"name":"purple"}))
                        else:
                            self.gui_signal.emit(json.dumps({"source":0,"name":"purple"}))
                        self.wait_over()


                    case 14:  # 移动七巧板
                        cmd = 13
                        posi = self.position["green"]
                        if posi !=[]:
                            posi = self.count_position(posi)
                            self.tcp_send(value=posi,cmd=cmd)
                            self.gui_signal.emit(json.dumps({"source":1,"name":"Kitchen"}))
                        else:
                            self.gui_signal.emit(json.dumps({"source":0,"name":"Kitchen"}))
                        self.wait_over()


                    case 15:  # 移动七巧板
                        cmd = 14
                        posi = self.position["yellow"]
                        if posi !=[]:
                            posi = self.count_position(posi)
                            self.tcp_send(value=posi,cmd=cmd)
                            self.gui_signal.emit(json.dumps({"source":1,"name":"Other"}))
                        else:
                            self.gui_signal.emit(json.dumps({"source":0,"name":"Other"}))
                        self.wait_over()



                    case 16:  # 移动七巧板
                        cmd = num
                        posi = self.position["orange"]
                        if posi !=[]:
                            posi = self.count_position(posi)
                            self.tcp_send(value=posi,cmd=cmd)
                            self.gui_signal.emit(json.dumps({"source":1,"name":"orange"}))
                        else:
                            self.gui_signal.emit(json.dumps({"source":0,"name":"orange"}))
                        self.wait_over()

                    case 17:  # 移动七巧板
                        cmd = num
                        posi = self.position["magenta"]
                        if posi !=[]:
                            posi = self.count_position(posi)
                            self.tcp_send(value=posi,cmd=cmd)
                            self.gui_signal.emit(json.dumps({"source":1,"name":"magenta"}))
                        else:
                            self.gui_signal.emit(json.dumps({"source":0,"name":"magenta"}))
                        self.wait_over()
                    
                    case 18:
                        self.auto_flag =0
                        self.action_flag = 0
                        self.gui_signal.emit(json.dumps({"source":3,}))
                    case 19:

                        cmd = 19
                        self.tcp_send(value=[10,10,10],cmd=cmd)   # 复位命令
                        self.action_flag = 0
                        self.camera_position = 0
                    case 20:

                        cmd = 20
                        self.tcp_send(value=[10,10,10],cmd=cmd)   # 移动到拍照位置
                        self.camera_position = 1
                        self.action_flag = 0

                    case 21:  # 移动圆形
                        cmd = 31
                        posi = self.position["circle"]
                        if posi !=[]:
                            posi = self.count_position(posi)
                            self.tcp_send(value=posi,cmd=cmd)
                            self.gui_signal.emit(json.dumps({"source":1,"name":"circle"}))
                        else:
                            self.gui_signal.emit(json.dumps({"source":0,"name":"circle"}))
                        self.wait_over()

                    case 22:  # 移动三角形
                        cmd = 32
                        posi = self.position["triangle"]
                        if posi !=[]:
                            posi = self.count_position(posi)
                            self.tcp_send(value=posi,cmd=cmd)
                            self.gui_signal.emit(json.dumps({"source":1,"name":"triangle"}))
                        else:
                            self.gui_signal.emit(json.dumps({"source":0,"name":"triangle"}))
                        self.wait_over()
                    
                    case 23:  # 移动六边形
                        cmd = 34
                        posi = self.position["heaxgon"]
                        if posi !=[]:
                            posi = self.count_position(posi)
                            self.tcp_send(value=posi,cmd=cmd)
                            self.gui_signal.emit(json.dumps({"source":1,"name":"heaxgon"}))
                        else:
                            self.gui_signal.emit(json.dumps({"source":0,"name":"heaxgon"}))
                        self.wait_over()

                    case 24:  # 移动矩形
                        cmd = 33
                        posi = self.position["rectangle"]
                        if posi !=[]:
                            posi = self.count_position(posi)
                            self.tcp_send(value=posi,cmd=cmd)
                            self.gui_signal.emit(json.dumps({"source":1,"name":"rectangle"}))
                        else:
                            self.gui_signal.emit(json.dumps({"source":0,"name":"rectangle"}))
                        self.wait_over()
                
                   
                    case 30:  # 移动到拍照位置
                        cmd = 10
                        self.tcp_send(value=[10,10,10],cmd=cmd)   #  移动到拍照位置
                        self.camera_position = 1
                        self.action_flag = 0


        except Exception as e:
            logger.error(e,exc_info=True)

    def run(self,):
        while self.run_flag:
            try:
                if self.run_flag:
                    time.sleep(0.1)
                    if self.action_flag>0:  # 如果有命令出现
                        val = self.master.execute(1,cst.READ_HOLDING_REGISTERS,0,1)
                        if val[0] != 1:
                            self.gui_signal.emit(json.dumps({"source":4,}))
                            if self.auto_flag ==0:
                                self.action_flag = 0
                        else:
                            if self.action_flag ==19: # 执行复位到home指令
                                self.action_run(self.action_flag)   # 执行相应的动作
                            else:
                                if self.camera_position ==0 or self.auto_first == 1 :
                                    self.gui_signal.emit(json.dumps({"source":5,}))
                                    self.action_run(num = 30)   # 执行对应的动作
                                    self.camera_position = 1
                                    self.auto_first = 0
                                    self.action_flag = 0
                                else :
                                    self.action_run(self.action_flag)   # 执行相应的动作

            except Exception as e:
                logger.error(e,exc_info=True)
